﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

//调用外部类
using ModbusCommunication;
//using System.Windows.Forms;


//这是一个专门处理UR数据的类，包括抓取数据以及存储数据，供主程序调用
namespace URDate
{

    class URDateHandle
    {
        //我需要定义几组全局的变量，用来实时存储当前的六个坐标值，角度值，以及机械手的状态等，以方便在任何时候调用
        
        //1 六个TCP坐标值
        public static double Positions_X;
        public static double Positions_Y;
        public static double Positions_Z;
        public static double Positions_U;
        public static double Positions_V;
        public static double Positions_W;

        //2 六个TCP速度值
        public static double Speed_X;
        public static double Speed_Y;
        public static double Speed_Z;
        public static double Speed_U;
        public static double Speed_V;
        public static double Speed_W;


        //3 六个关节角度值
        public static double Angles_X;
        public static double Angles_Y;
        public static double Angles_Z;
        public static double Angles_U;
        public static double Angles_V;
        public static double Angles_W;

        //4 六个关节角速度值
        public static double AnglesSpeed_X;
        public static double AnglesSpeed_Y;
        public static double AnglesSpeed_Z;
        public static double AnglesSpeed_U;
        public static double AnglesSpeed_V;
        public static double AnglesSpeed_W;

        //5 六个关节温度值
        public static double AnglesTemperature_X;
        public static double AnglesTemperature_Y;
        public static double AnglesTemperature_Z;
        public static double AnglesTemperature_U;
        public static double AnglesTemperature_V;
        public static double AnglesTemperature_W;

        //6 六个关节电流值
        public static double AnglesCurrent_X;
        public static double AnglesCurrent_Y;
        public static double AnglesCurrent_Z;
        public static double AnglesCurrent_U;
        public static double AnglesCurrent_V;
        public static double AnglesCurrent_W;

        //7 六个关节模式
        public static double JointMode_X;
        public static double JointMode_Y;
        public static double JointMode_Z;
        public static double JointMode_U;
        public static double JointMode_V;
        public static double JointMode_W;

        //8 TCP的六个位置补偿值
        public static double TCPOffset_X;
        public static double TCPOffset_Y;
        public static double TCPOffset_Z;
        public static double TCPOffset_U;
        public static double TCPOffset_V;
        public static double TCPOffset_W;

        //9 机器人状态（是否上电/安全停机等）
        public static double RobotStatus0;
        public static double RobotStatus1;
        public static double RobotStatus2;
        public static double RobotStatus3;
        public static double RobotStatus4;
        public static double RobotStatus5;

        //10 机器人运行时间（天/小时/分钟等）
        public static double RobotRuntimeSplittime;
        public static double RobotRuntimeMilliseconds;
        public static double RobotRuntimeSeconds;
        public static double RobotRuntimeMinutes;
        public static double RobotRuntimeHours;
        public static double RobotRuntimeDays;

        //11 控制器版本
        public static double ControllerVersionHigh;
        public static double ControllerVersionLow;

        //12 机器人电流
        public static double Current0;
        public static double Current1;

        //13 工具端状态
        public static double ToolState0;
        public static double ToolState1;
        public static double ToolState2;


        public static void Show()
        {
            //Console.WriteLine("Positions\t"+ Positions_X+'\t'+ Positions_Y + '\t' + Positions_Z + '\t' +
            //    Positions_U + '\t' + Positions_V + '\t' + Positions_W + '\t' );

            //Console.WriteLine("Speed\t" + Speed_X + '\t' + Speed_Y + '\t' + Speed_Z + '\t' +
            //Speed_U + '\t' + Speed_V + '\t' + Speed_W + '\t');


            //Console.WriteLine("Angles\t" + Angles_X + '\t' + Angles_Y + '\t' + Angles_Z + '\t' +
            //    Angles_U + '\t' + Angles_V + '\t' + Angles_W + '\t');


            //Console.WriteLine("AnglesSpeed\t" + AnglesSpeed_X + '\t' + AnglesSpeed_Y + '\t' + AnglesSpeed_Z + '\t' +
            //    AnglesSpeed_U + '\t' + AnglesSpeed_V + '\t' + AnglesSpeed_W + '\t');


            //Console.WriteLine("AnglesTemperature\t" + AnglesTemperature_X + '\t' + AnglesTemperature_Y + '\t' + AnglesTemperature_Z + '\t' +
            //    AnglesTemperature_U + '\t' + AnglesTemperature_V + '\t' + AnglesTemperature_W + '\t');


            //Console.WriteLine("AnglesCurrent\t" + AnglesCurrent_X + '\t' + AnglesCurrent_Y + '\t' + AnglesCurrent_Z + '\t' +
            //    AnglesCurrent_U + '\t' + AnglesCurrent_V + '\t' + AnglesCurrent_W + '\t');


            Console.WriteLine("JointMode\t" + JointMode_X + '\t' + JointMode_Y + '\t' + JointMode_Z + '\t' +
                JointMode_U + '\t' + JointMode_V + '\t' + JointMode_W + '\t');


            //Console.WriteLine("TCPOffset\t" + TCPOffset_X + '\t' + TCPOffset_Y + '\t' + TCPOffset_Z + '\t' +
            //    TCPOffset_U + '\t' + TCPOffset_V + '\t' + TCPOffset_W + '\t');

        }



        //声明分隔符
        public static string splitTab = " ";

        //声明长度为2054的double类型数组，每次都会刷新对应的数据
        public static double[] URDataPackageArray = new double[2054];

        //申明刷新速度
        public static int ScanRate;

        //在这里生成一个Modbus的Socket
        ModbusSocket Socket502_TCP = new ModbusSocket();

        //每一组类的实例分开来
        ModbusSocket Socket502_Angle = new ModbusSocket();

        //每一组类的实例分开来
        ModbusSocket Socket502_URState = new ModbusSocket();

        //前面的Socket502_URState去读取官方的文档，往往只能获取机器人的大致状态，我需要获取机器人是否在运动，暂时通过判断六个速度寄存器来粗略判断
        ModbusSocket Socket502_URSpeedState = new ModbusSocket();

        //在这里申明一个定时器，用于实时获取UR数据
        public System.Timers.Timer GetRobotInformationTimer;

        //申明第1组委托(把收到的六个TCP坐标值传递出去)
        public delegate void GetTCPPositionSuccess(double[] PositionArray);
        public event GetTCPPositionSuccess OnGetTCPPositionSuccess;

        //申明第2组委托(把收到的六个角度坐标值传递出去)
        public delegate void GetJointAngleSuccess(double[] AngleArray);
        public event GetJointAngleSuccess OnGetJointAngleSuccess;

        //申明第3组委托(把收到的六个TCP速度值传递出去)
        public delegate void GetTCPSpeedSuccess(double[] TCPSpeed);
        public event GetTCPSpeedSuccess OnGetTCPSpeedSuccess;

        //申明第4组委托(把收到的六个角速度值传递出去)
        public delegate void GetJointSpeedSuccess(double[] JointSpeed);
        public event GetJointSpeedSuccess OnGetJointSpeedSuccess;

        //申明第5组委托(把收到的六个机械手状态值传递出去)
        public delegate void GetRobotStateSuccess(int[] RobotState);
        public event GetRobotStateSuccess OnGetRobotStateSuccess;

        //初始化监视器的方法
        public void InitialMoniter(string UR_IP)
        {
            //初始化这个Socket502_TCP端口
            Socket502_TCP.RemoteIP = UR_IP;
            Socket502_TCP.RemotePort = 502;
            Socket502_TCP.SocketTimeOut = 1000;
            Socket502_TCP.initialServer();

            //初始化这个Socket502_Angle端口
            Socket502_Angle.RemoteIP = UR_IP;
            Socket502_Angle.RemotePort = 502;
            Socket502_Angle.SocketTimeOut = 1000;
            Socket502_Angle.initialServer();

            //初始化这个Socket502_URState端口
            Socket502_URState.RemoteIP = UR_IP;
            Socket502_URState.RemotePort = 502;
            Socket502_URState.SocketTimeOut = 1000;
            Socket502_URState.initialServer();

            //初始化这个Socket502_URSpeedState端口
            Socket502_URSpeedState.RemoteIP = UR_IP;
            Socket502_URSpeedState.RemotePort = 502;
            Socket502_URSpeedState.SocketTimeOut = 1000;
            Socket502_URSpeedState.initialServer();

            //调用定时器，在线程中定时执行任务
            GetRobotInformationTimer = new System.Timers.Timer(ScanRate);
            GetRobotInformationTimer.Elapsed += new System.Timers.ElapsedEventHandler(GetRobotInformation);
            GetRobotInformationTimer.AutoReset = true;//设置是执行一次（false）还是一直执行(true)；
            GetRobotInformationTimer.Enabled = true;//是否执行System.Timers.Timer.Elapsed事件；

        }


        //初始化监视器的方法
        public void InitialMoniterA(string UR_IP)
        {
            //初始化这个Socket502_TCP端口
            Socket502_TCP.RemoteIP = UR_IP;
            Socket502_TCP.RemotePort = 502;
            Socket502_TCP.SocketTimeOut = 1000;
            Socket502_TCP.initialServer();

            //初始化这个Socket502_Angle端口
            Socket502_Angle.RemoteIP = UR_IP;
            Socket502_Angle.RemotePort = 502;
            Socket502_Angle.SocketTimeOut = 1000;
            Socket502_Angle.initialServer();

            //初始化这个Socket502_URState端口
            Socket502_URState.RemoteIP = UR_IP;
            Socket502_URState.RemotePort = 502;
            Socket502_URState.SocketTimeOut = 1000;
            Socket502_URState.initialServer();

            //初始化这个Socket502_URSpeedState端口
            Socket502_URSpeedState.RemoteIP = UR_IP;
            Socket502_URSpeedState.RemotePort = 502;
            Socket502_URSpeedState.SocketTimeOut = 1000;
            Socket502_URSpeedState.initialServer();

            ////调用定时器，在线程中定时执行任务
            //GetRobotInformationTimer = new System.Timers.Timer(ScanRate);
            //GetRobotInformationTimer.Elapsed += new System.Timers.ElapsedEventHandler(GetRobotInformation);
            //GetRobotInformationTimer.AutoReset = true;//设置是执行一次（false）还是一直执行(true)；
            //GetRobotInformationTimer.Enabled = true;//是否执行System.Timers.Timer.Elapsed事件；

        }


        //这里定时抓取UR信息
        public void GetRobotInformationA()
        {
            //获取TCP位置和关节位置
            GetTCPPositions();

            //Console.WriteLine("GetTCPPositions");

            GetJointAngles();

            //获取TCP速度和关节速度
            GetTCPSpeed();
            GetJointSpeed();

            //获取关节电流和关节温度
            GetJointCurrent();
            GetJointTemperature();

            //获取关节模式
            GetJointMode();

            //获取TCP偏移
            GetTCPOffset();

            //获取机器人状态
            GetRobotState();

            //获取运行时间
            GetRuntime();

            //获取机器人版本
            GetVersion();

            //获取机器人电流
            GetRobotCurrent();

            //获取工具端状态
            GetToolStatus();

            //获取人机界面状态
            GetInterfaceStatus();

            //Console.WriteLine("GetInterfaceStatus");

            Show();
        }



        //这里定时抓取UR信息
        public void GetRobotInformation(object source, System.Timers.ElapsedEventArgs e)
        {
            //获取TCP位置和关节位置
            GetTCPPositions();

            //Console.WriteLine("GetTCPPositions");

            GetJointAngles();

            //获取TCP速度和关节速度
            GetTCPSpeed();
            GetJointSpeed();

            //获取关节电流和关节温度
            GetJointCurrent();
            GetJointTemperature();

            //获取关节模式
            GetJointMode();

            //获取TCP偏移
            GetTCPOffset();

            //获取机器人状态
            GetRobotState();

            //获取运行时间
            GetRuntime();

            //获取机器人版本
            GetVersion();

            //获取机器人电流
            GetRobotCurrent();

            //获取工具端状态
            GetToolStatus();
            
            //获取人机界面状态
            GetInterfaceStatus();

            //Console.WriteLine("GetInterfaceStatus");

            Show();
        }

        //1 取得TCP坐标值方法(从400-405寄存器读取) OK
        public void GetTCPPositions()
        {
            //这里实时读取UR数据（先读取六个坐标值，手册来看地址就是400）
            int[] SixPositions = Socket502_TCP.ReadMultipleRegister(6, 400,false);

            //对于XYZ，取得的并不能直接使用，而是要经过转换
            double[] PositionFiltered = new double[6];

            for (int i = 0; i < 6; i++)
            {
                //见Modbus UR参考文档：如果XYZ有超过32768，则变成负值（虽然是0-65535，但是我们要自己变成有符号数“-32767”到“32768”）
                if (SixPositions[i] > 32768)
                {
                    SixPositions[i] = SixPositions[i] - 65535;
                }

                //前面三个XYZ做同样处理(对于所有的XYZ都要除以10)
                if (i < 3)
                {
                    PositionFiltered[i] = (float)SixPositions[i] / 10;
                }
                //后面三个UVW只要直接除以1000即可
                else 
                {
                    PositionFiltered[i] = (float)SixPositions[i] / 1000;
                }
            }

            //读到之后就改写自己的成员值(注意XYZ值还是要原汁原味的浮点数，方便计算)
            URDateHandle.Positions_X = (PositionFiltered[0] / 1000);
            URDateHandle.Positions_Y = (PositionFiltered[1] / 1000);
            URDateHandle.Positions_Z = (PositionFiltered[2] / 1000);

            URDateHandle.Positions_U = (PositionFiltered[3]);
            URDateHandle.Positions_V = (PositionFiltered[4]);
            URDateHandle.Positions_W = (PositionFiltered[5]);

            URDataPackageArray[400] = PositionFiltered[0];
            URDataPackageArray[401] = PositionFiltered[1];
            URDataPackageArray[402] = PositionFiltered[2];
            URDataPackageArray[403] = PositionFiltered[3];
            URDataPackageArray[404] = PositionFiltered[4];
            URDataPackageArray[405] = PositionFiltered[5];

            //还是需要借助委托触发，我这里执行完了，怎么通知主程序是关键
            //OnGetTCPPositionSuccess(PositionFiltered);

        }

        //2 取得TCP速度值方法(从410-415寄存器读取) OK
        public void GetTCPSpeed()
        {
            int[] SixSpeeds = Socket502_TCP.ReadMultipleRegister(6, 410, false);

            double[] SpeedFiltered = new double[6];

            for (int i = 0; i < 6; i++)
            {
                if (SixSpeeds[i] > 32768)
                {
                    SixSpeeds[i] = SixSpeeds[i] - 65535;
                }

                //TCP速度不需要除以
                if (i < 3)
                {
                    SpeedFiltered[i] = (double)SixSpeeds[i];
                }
                //但是后面三个默认是以弧度每秒为单位的，要转换成度每秒
                else
                {
                    SpeedFiltered[i] = (double)SixSpeeds[i]/1000 * (180 / 3.14);
                }


            }

            URDateHandle.Speed_X = (SpeedFiltered[0]);
            URDateHandle.Speed_Y = (SpeedFiltered[1]);
            URDateHandle.Speed_Z = (SpeedFiltered[2]);

            URDateHandle.Speed_U = (SpeedFiltered[3]);
            URDateHandle.Speed_V = (SpeedFiltered[4]);
            URDateHandle.Speed_W = (SpeedFiltered[5]);


            URDataPackageArray[410] = URDateHandle.Speed_X;
            URDataPackageArray[411] = URDateHandle.Speed_Y;
            URDataPackageArray[412] = URDateHandle.Speed_Z;
            URDataPackageArray[413] = URDateHandle.Speed_U;
            URDataPackageArray[414] = URDateHandle.Speed_V;
            URDataPackageArray[415] = URDateHandle.Speed_W;

            //还是需要借助委托触发，我这里执行完了，怎么通知主程序是关键
            //OnGetTCPSpeedSuccess(SpeedFiltered);
        }

        //3 取得六个角度值的方法(从270-275寄存器读取) OK
        public void GetJointAngles()
        {
            //这里实时读取UR数据（读取六个关节角度值，手册来看地址就是270）
            int[] SixAngles = Socket502_Angle.ReadMultipleRegister(6, 270, false);

            //对于六个角度值，只有double类型可以放这种双精度类型值
            double[] AnglesFiltered = new double[6];

            for (int i = 0; i < 6; i++)
            {
                if (SixAngles[i] > 32768)
                {
                    SixAngles[i] = SixAngles[i] - 65535;
                }

                //这里取到的都是弧度值，要转换为角度值(除以1000之后还是弧度，要再转换为角度)
                AnglesFiltered[i] = (double)SixAngles[i] / 1000 * (180 / 3.14);

                if(i!=4)
                {
                     AnglesFiltered[i] = AnglesFiltered[i]-360;
                }

            }


            URDateHandle.Angles_X = (AnglesFiltered[0] * (Math.PI / 180));
            URDateHandle.Angles_Y = (AnglesFiltered[1] * (Math.PI / 180));
            URDateHandle.Angles_Z = (AnglesFiltered[2] * (Math.PI / 180));
            URDateHandle.Angles_U = (AnglesFiltered[3] * (Math.PI / 180));
            URDateHandle.Angles_V = (AnglesFiltered[4] * (Math.PI / 180));
            URDateHandle.Angles_W = (AnglesFiltered[5] * (Math.PI / 180));


            URDataPackageArray[270] = AnglesFiltered[0];
            URDataPackageArray[271] = AnglesFiltered[1];
            URDataPackageArray[272] = AnglesFiltered[2];
            URDataPackageArray[273] = AnglesFiltered[3];
            URDataPackageArray[274] = AnglesFiltered[4];
            URDataPackageArray[275] = AnglesFiltered[5];

            //还是需要借助委托触发，我这里执行完了，怎么通知主程序是关键
            //OnGetJointAngleSuccess(AnglesFiltered);

        }

        //4 取得六个角速度值的方法(从280-285寄存器读取) OK
        public void GetJointSpeed()
        {
            int[] RobotSpeed = Socket502_URSpeedState.ReadMultipleRegister(6, 280, false);

            double[] SpeedFiltered = new double[6];

            for (int i = 0; i < 6; i++)
            {
                if (RobotSpeed[i] > 32768)
                {
                    RobotSpeed[i] = RobotSpeed[i] - 65535;
                }

                if (i < 3)
                {
                    SpeedFiltered[i] = (float)RobotSpeed[i] / 1000 * (180 / 3.14);
                }
                else
                {
                    SpeedFiltered[i] = (float)RobotSpeed[i] / 1000 * (180 / 3.14);
                }

            }

            URDateHandle.AnglesSpeed_X = SpeedFiltered[0];
            URDateHandle.AnglesSpeed_Y = SpeedFiltered[1];
            URDateHandle.AnglesSpeed_Z = SpeedFiltered[2];

            URDateHandle.AnglesSpeed_U = (SpeedFiltered[3]);
            URDateHandle.AnglesSpeed_V = (SpeedFiltered[4]);
            URDateHandle.AnglesSpeed_W = (SpeedFiltered[5]);

            URDataPackageArray[280] = URDateHandle.AnglesSpeed_X;
            URDataPackageArray[281] = URDateHandle.AnglesSpeed_Y;
            URDataPackageArray[282] = URDateHandle.AnglesSpeed_Z;
            URDataPackageArray[283] = URDateHandle.AnglesSpeed_U;
            URDataPackageArray[284] = URDateHandle.AnglesSpeed_V;
            URDataPackageArray[285] = URDateHandle.AnglesSpeed_W;


            //OnGetJointSpeedSuccess(SpeedFiltered);
        }

        //5 取得六个关节温度的方法(从300-305寄存器读取) OK
        public void GetJointTemperature()
        {
            int[] RobotSpeed = Socket502_URSpeedState.ReadMultipleRegister(6, 300, false);

            double[] SpeedFiltered = new double[6];

            for (int i = 0; i < 6; i++)
            {
                if (RobotSpeed[i] > 32768)
                {
                    RobotSpeed[i] = RobotSpeed[i] - 65535;
                }

                SpeedFiltered[i] = (double)RobotSpeed[i];
            }

            URDateHandle.AnglesTemperature_X = SpeedFiltered[0];
            URDateHandle.AnglesTemperature_Y = SpeedFiltered[1];
            URDateHandle.AnglesTemperature_Z = SpeedFiltered[2];

            URDateHandle.AnglesTemperature_U = (SpeedFiltered[3]);
            URDateHandle.AnglesTemperature_V = (SpeedFiltered[4]);
            URDateHandle.AnglesTemperature_W = (SpeedFiltered[5]);

            URDataPackageArray[300] = URDateHandle.AnglesTemperature_X;
            URDataPackageArray[301] = URDateHandle.AnglesTemperature_Y;
            URDataPackageArray[302] = URDateHandle.AnglesTemperature_Z;
            URDataPackageArray[303] = URDateHandle.AnglesTemperature_U;
            URDataPackageArray[304] = URDateHandle.AnglesTemperature_V;
            URDataPackageArray[305] = URDateHandle.AnglesTemperature_W;

        }

        //6 取得六个关节电流的方法(从290-295寄存器读取) OK
        public void GetJointCurrent()
        {
            int[] RobotSpeed = Socket502_URSpeedState.ReadMultipleRegister(6, 290, false);

            double[] SpeedFiltered = new double[6];

            for (int i = 0; i < 6; i++)
            {
                if (RobotSpeed[i] > 32768)
                {
                    RobotSpeed[i] = RobotSpeed[i] - 65535;
                }

                SpeedFiltered[i] = (double)RobotSpeed[i] / 1000;
            }

            URDateHandle.AnglesCurrent_X = SpeedFiltered[0];
            URDateHandle.AnglesCurrent_Y = SpeedFiltered[1];
            URDateHandle.AnglesCurrent_Z = SpeedFiltered[2];

            URDateHandle.AnglesCurrent_U = (SpeedFiltered[3]);
            URDateHandle.AnglesCurrent_V = (SpeedFiltered[4]);
            URDateHandle.AnglesCurrent_W = (SpeedFiltered[5]);


            URDataPackageArray[290] = URDateHandle.AnglesCurrent_X;
            URDataPackageArray[291] = URDateHandle.AnglesCurrent_Y;
            URDataPackageArray[292] = URDateHandle.AnglesCurrent_Z;
            URDataPackageArray[293] = URDateHandle.AnglesCurrent_U;
            URDataPackageArray[294] = URDateHandle.AnglesCurrent_V;
            URDataPackageArray[295] = URDateHandle.AnglesCurrent_W;


        }

        //7 取得六个关节模式的方法(从310-315寄存器读取) OK
        public void GetJointMode()
        {
            int[] RobotSpeed = Socket502_URSpeedState.ReadMultipleRegister(6, 310, false);

            double[] SpeedFiltered = new double[6];

            for (int i = 0; i < 6; i++)
            {
                if (RobotSpeed[i] > 32768)
                {
                    RobotSpeed[i] = RobotSpeed[i] - 65535;
                }

                SpeedFiltered[i] = (double)RobotSpeed[i] / 1;
            }

            URDateHandle.JointMode_X = SpeedFiltered[0];
            URDateHandle.JointMode_Y = SpeedFiltered[1];
            URDateHandle.JointMode_Z = SpeedFiltered[2];

            URDateHandle.JointMode_U = (SpeedFiltered[3]);
            URDateHandle.JointMode_V = (SpeedFiltered[4]);
            URDateHandle.JointMode_W = (SpeedFiltered[5]);

            URDataPackageArray[310] = URDateHandle.JointMode_X;
            URDataPackageArray[311] = URDateHandle.JointMode_Y;
            URDataPackageArray[312] = URDateHandle.JointMode_Z;
            URDataPackageArray[313] = URDateHandle.JointMode_U;
            URDataPackageArray[314] = URDateHandle.JointMode_V;
            URDataPackageArray[315] = URDateHandle.JointMode_W;

        }

        //8 取得六个TCP补偿值的方法(从420-425寄存器读取) OK
        public void GetTCPOffset()
        {
            int[] RobotSpeed = Socket502_URSpeedState.ReadMultipleRegister(6, 420, false);

            double[] SpeedFiltered = new double[6];

            for (int i = 0; i < 6; i++)
            {
                if (RobotSpeed[i] > 32768)
                {
                    RobotSpeed[i] = RobotSpeed[i] - 65535;
                }

                if (i < 3)
                {
                    SpeedFiltered[i] = (double)RobotSpeed[i] / 1;
                }
                else
                {
                    SpeedFiltered[i] = (double)RobotSpeed[i] / 1000;
                }

            }

            URDateHandle.TCPOffset_X = SpeedFiltered[0];
            URDateHandle.TCPOffset_Y = SpeedFiltered[1];
            URDateHandle.TCPOffset_Z = SpeedFiltered[2];

            URDateHandle.TCPOffset_U = (SpeedFiltered[3]);
            URDateHandle.TCPOffset_V = (SpeedFiltered[4]);
            URDateHandle.TCPOffset_W = (SpeedFiltered[5]);

            URDataPackageArray[420] = URDateHandle.TCPOffset_X;
            URDataPackageArray[421] = URDateHandle.TCPOffset_Y;
            URDataPackageArray[422] = URDateHandle.TCPOffset_Z;
            URDataPackageArray[423] = URDateHandle.TCPOffset_U;
            URDataPackageArray[424] = URDateHandle.TCPOffset_V;
            URDataPackageArray[425] = URDateHandle.TCPOffset_W;


        }

        //9 取得机器人的运行状态(从260-265寄存器读取) OK
        public void GetRobotState()
        {
            //这里实时读取UR数据（读取是否开机，是否紧急停机等状态信息，手册来看地址就是260）
            int[] RobotState = Socket502_URState.ReadMultipleRegister(6, 260, false);

            URDateHandle.RobotStatus0 = RobotState[0];
            URDateHandle.RobotStatus1 = RobotState[1];
            URDateHandle.RobotStatus2 = RobotState[2];
            URDateHandle.RobotStatus3 = RobotState[3];
            URDateHandle.RobotStatus4 = RobotState[4];
            URDateHandle.RobotStatus5 = RobotState[5];

            URDataPackageArray[260] = URDateHandle.RobotStatus0;
            URDataPackageArray[261] = URDateHandle.RobotStatus1;
            URDataPackageArray[262] = URDateHandle.RobotStatus2;
            URDataPackageArray[263] = URDateHandle.RobotStatus3;
            URDataPackageArray[264] = URDateHandle.RobotStatus4;
            URDataPackageArray[265] = URDateHandle.RobotStatus5;


            //OnGetRobotStateSuccess(RobotState);
        }

        //10 获取机器人运行时间(从2048-2053寄存器读取) OK
        public void GetRuntime()
        {
            int[] RobotSpeed = Socket502_URSpeedState.ReadMultipleRegister(6, 2048, false);

            double[] SpeedFiltered = new double[6];

            for (int i = 0; i < 6; i++)
            {
                if (RobotSpeed[i] > 32768)
                {
                    RobotSpeed[i] = RobotSpeed[i] - 65535;
                }

                if (i < 3)
                {
                    SpeedFiltered[i] = (double)RobotSpeed[i] / 1;
                }
                else
                {
                    SpeedFiltered[i] = (double)RobotSpeed[i] / 1;
                }

            }

            URDateHandle.RobotRuntimeSplittime = SpeedFiltered[0];
            URDateHandle.RobotRuntimeMilliseconds = SpeedFiltered[1];
            URDateHandle.RobotRuntimeSeconds = SpeedFiltered[2];

            URDateHandle.RobotRuntimeMinutes = (SpeedFiltered[3]);
            URDateHandle.RobotRuntimeHours = (SpeedFiltered[4]);
            URDateHandle.RobotRuntimeDays = (SpeedFiltered[5]);


            URDataPackageArray[2048] = URDateHandle.RobotRuntimeSplittime;
            URDataPackageArray[2049] = URDateHandle.RobotRuntimeMilliseconds;
            URDataPackageArray[2050] = URDateHandle.RobotRuntimeSeconds;
            URDataPackageArray[2051] = URDateHandle.RobotRuntimeMinutes;
            URDataPackageArray[2052] = URDateHandle.RobotRuntimeHours;
            URDataPackageArray[2053] = URDateHandle.RobotRuntimeDays;


        }

        //11 获取机器人版本(从256-257寄存器读取) OK
        public void GetVersion()
        {
            int[] RobotSpeed = Socket502_URSpeedState.ReadMultipleRegister(3, 256, false);

            URDateHandle.ControllerVersionHigh = RobotSpeed[0];
            URDateHandle.ControllerVersionLow = RobotSpeed[1];

            URDataPackageArray[256] = URDateHandle.ControllerVersionHigh;
            URDataPackageArray[257] = URDateHandle.ControllerVersionLow;
            URDataPackageArray[258] = RobotSpeed[2];

        }

        //12 获取机器人电流(从450-451寄存器读取) OK
        public void GetRobotCurrent()
        {
            int[] RobotSpeed = Socket502_URSpeedState.ReadMultipleRegister(2, 450, false);

            double[] SpeedFiltered = new double[2];

            for (int i = 0; i < 2; i++)
            {
                if (RobotSpeed[i] > 32768)
                {
                    RobotSpeed[i] = RobotSpeed[i] - 65535;
                }

                SpeedFiltered[i] = (double)RobotSpeed[i] / 1000;
            }

            URDateHandle.Current0 = SpeedFiltered[0];
            URDateHandle.Current1 = SpeedFiltered[1];

            URDataPackageArray[450] = URDateHandle.Current0;
            URDataPackageArray[451] = URDateHandle.Current1;
        }

        //13 获取工具端状态(从768-770寄存器读取) OK
        public void GetToolStatus()
        {
            int[] RobotSpeed = Socket502_URSpeedState.ReadMultipleRegister(3, 768, false);

            double[] SpeedFiltered = new double[3];

            for (int i = 0; i < 3; i++)
            {
                if (RobotSpeed[i] > 32768)
                {
                    RobotSpeed[i] = RobotSpeed[i] - 65535;
                }

                if (i == 0)
                {
                    SpeedFiltered[i] = (double)RobotSpeed[i] / 1;
                }
                else
                {
                    SpeedFiltered[i] = (double)RobotSpeed[i] / 1000;
                }
            }

            URDateHandle.ToolState0 = SpeedFiltered[0];
            URDateHandle.ToolState1 = SpeedFiltered[1];
            URDateHandle.ToolState2 = SpeedFiltered[2];

            URDataPackageArray[768] = URDateHandle.ToolState0;
            URDataPackageArray[769] = URDateHandle.ToolState1;
            URDataPackageArray[770] = URDateHandle.ToolState2;

        }

        //14 获取人机界面状态(从1024寄存器读取) OK
        public void GetInterfaceStatus()
        {
            int[] RobotSpeed = Socket502_URSpeedState.ReadMultipleRegister(1, 1024, false);
            URDataPackageArray[1024] = RobotSpeed[0];

        }

    }
}
